import math


def angle(v1):
    dx1 = v1[2] - v1[0]
    dy1 = v1[3] - v1[1]
    angle1 = math.atan2(dy1, dx1)
    if angle1 >= 0:
        return angle1
    else:
        return 2 * math.pi + angle1


def dad_op(segment):
    if len(segment) <= 2:
        # print('segment error', 0.0)
        return -1, 0.0
    else:
        mid = -1
        ps = segment[0]
        pe = segment[-1]
        e = 0.0
        theta_0 = angle([ps[0], ps[1], pe[0], pe[1]])
        for i in range(0, len(segment) - 1):
            pm_0 = segment[i]
            pm_1 = segment[i + 1]
            theta_1 = angle([pm_0[0], pm_0[1], pm_1[0], pm_1[1]])
            tmp = min(abs(theta_0 - theta_1), 2 * math.pi - abs(theta_0 - theta_1))
            if tmp > e:
                e = tmp
                mid = i
        # print('segment error', e)
        # if len(segment) == 3:
        #     mid = 1
        return mid, e


def getSED4GPS(p, start, end):
    (x, y), syn_time = p
    (st_x, st_y), st_time = start
    (en_x, en_y), en_time = end

    time_ratio = 1 if (st_time - en_time) == 0 else (syn_time - st_time) / (en_time - st_time)
    syn_x = st_x + (en_x - st_x) * time_ratio
    syn_y = st_y + (en_y - st_y) * time_ratio

    dx = x - syn_x
    dy = y - syn_y
    return math.sqrt(dx * dx + dy * dy)


def getPED4GPS(p, start, end):
    x, y, time = p[0], p[1], p[2]
    st_x, st_y, st_time = start[0], start[1], start[2]
    en_x, en_y, en_time = end[0], end[1], end[2]
    #     Ax + By + C = 0
    # PED
    if en_x == st_x:
        return abs(x - en_x)
    A = (en_y - st_y) / (en_x - st_x)
    B = -1
    C = st_y - st_x * A
    return abs(A * x + B * y + C) / math.sqrt(A * A + B * B)


def getError4GPS(p, start, end, mode):
    if mode == "dad":
        mid, e = dad_op([start, p, end])
        return e
    elif mode == "ped":
        return getPED4GPS(p, start, end)
    elif mode == "sed":
        return getSED4GPS(p, start, end)


def getMaxError(st, en, points, mode):
    if mode == 'dad':
        mid, e = dad_op(points[st:en + 1])
        return mid + st, e
    maxErr = -1
    idx = -1
    err = -1
    for i in range(st, en + 1):
        if mode == 'ped':
            err = getPED4GPS(points[i], points[st], points[en])
        elif mode == 'sed':
            err = getSED4GPS((points[i], i), (points[st], st), (points[en], en))
        if maxErr < err:
            maxErr = err
            idx = i
    return idx, maxErr


def calculate_error(points, seg1, seg2, mode):
    st = points[seg1[0]]
    en = points[seg2[-1]]
    max_err = -1
    if mode == 'dad':
        return getMaxError(seg1[0], seg2[-1], points, mode)[1]
    for i in range(seg1[0] + 1, seg2[-1]):
        p = points[i]
        if mode == 'ped':
            max_err = max(max_err, getPED4GPS(p, st, en))
        elif mode == 'sed':
            max_err = max(max_err, getSED4GPS((p, i), (st, seg1[0]), (en, seg2[-1])))
    return max_err
